Repository navigation
fix: read fresh status before motion and gate the grasp handover - #86
Conversation
cao-xiao-hao
left a comment
There was a problem hiding this comment.
Verdict: request changes
The diagnosis is right, and draining the status queue is the correct root-cause fix for the stale-cache path. Two things block merge: the behavioural changes have no tests, and the commit/PR metadata violate the repository rules. The technical points below are worth resolving before this lands.
Blocking
-
No tests for either behaviour change.
MotorController.poll()no longer consumes one frame per call. Only a queued-frames test proves that the newest state wins; the currenttests/fake_can.pycannot queue frames.MotionConfig.stall_speed_mm_sis a new safety gate. The false-stop case (free travel at ~8 mm/s against a 50 mm/s schedule) is not covered by any test.
Every new behaviour and every bug fix must ship with a test. The 498 existing tests passing does not exercise either change.
-
Commit message. The subject is 82 characters (limit 72), and the footer carries
Co-Authored-By: Claude Code <noreply@anthropic.com>. Agent attribution is not allowed. The PR body's "Generated with Claude Code" line should go too. -
PR body has no
ChangesorIssuessection. Add them, withRefs #…/Closes #…if there is an issue. -
CI has not run. Run 44261468705 is
action_required, so there is no test gate on this PR. It needs a run before merge.
Technical
-
poll_until()can miss an update with more than one motor.poll()now returns only the newest motor whose frame was drained, andpoll_until(motor)accepts the result only when it ismotor. If another registered motor's frame is drained last, the target's update is invisible and the call times out even thoughmotor.rx_countadvanced. Reproduced with two registered motors and a queue[A, B]:poll_until(A, timeout_s=0.05) -> False | A.rx_count 0 -> 1The single-motor
LiteGrippath is unaffected, butMotorControlleris public and supports multiple motors. Either expose every updated motor frompoll(), or havepoll_until()testmotor.rx_count > prev_rxregardless of the returned motor. -
The known limitation sits on a normal operating point. At 2.5 N the free creep is ~1.9 mm/s, just below the new 2.0 mm/s gate, so the jaws can still be handed to the torque-only hold while still creeping. Reproducing at a normal grasp force means the threshold has no margin. Either include the stricter package (0.5 mm/s plus two confirm windows) or derive the gate from the force budget instead of a bench-tuned constant.
-
The final reach check still reads the cache.
actions.py:1852still callsg.get_state()rather than_fresh_state().get_state(wait=True)returns the cached reading after 50 ms when nobody answers, and the code does not inspectfresh, soreached/okcan be decided from a stale position — the class of bug this PR fixes. -
"Never plan from a stale cache" is weaker than the docstring says. When
refresh_status()succeeds but no frame arrives,_fresh_state()accepts a cache up to theSTALE_AFTER_Sdefault (0.5 s) old. At the 20 mm/s open/close speed that is up to 10 mm of error. Consider a tighter threshold for motion planning, or requirefresh. -
grasp()error contract changes._hold_force()now calls_fresh_state(), which can raiseCommErrormid-hold.grasp()documents onlyCommandError, and itsfinallyclears the release state without releasing torque. If callers are expected to handle an exception instead ofHoldResult(ok=False), document it; otherwise convert it to a failed hold.
Nits
poll()drains untilrecv()returnsNone; on a bus that keeps delivering, the loop is bounded only by the bus. A maximum-drain cap would bound the added latency.stall_speed_mm_sis not validated. A negative value makesmovingalways true, so the approach always refuses at the ceiling — safe, but silent.
Testing performed
python3 -m unittest discover -s tests -t tests on 840545a: 498 tests, OK.
The motor answers every control frame with a status frame. Reading one frame per call left the rest queued, and the age was stamped when a frame was read, so an old position looked fresh. poll() now drains the queue (bounded) and lets the newest frame define the state. poll_until() checks the target's rx_count instead of the motor returned by the drain, so another registered motor's frame cannot hide the update. GripperActions._fresh_state() requests a status frame (0xCC) before every motion, retries, and refuses to plan when no frame answers. A cached snapshot -- even one younger than STALE_AFTER_S -- is not enough: at 20 mm/s, half a second of silence is 10 mm of unknown. The final reach check now uses this path too, so reached/ok are not decided from the pre-loop cache. The force-approach handover also requires the window's average speed to be below MotionConfig.stall_speed_mm_s (2 mm/s). A force-budgeted approach that crawls (~8 mm/s on this bench) is not contact; handing it to the torque-only hold accelerates the jaws across the free stroke. The threshold stays at 2 mm/s: the 2.5 N reproduction is below the documented 3 N setpoint, where the grip is already marginal (README). A mid-hold CommError is converted to GraspResult(ok=False) after a zero-torque release, instead of leaving the last hold torque on the bus. stall_speed_mm_s is validated, and the poll drain is capped. Tests cover the drain and newest-wins behaviour, the bounded drain, multi-motor poll_until, the slow creep that must not be handed over, an idle bus that must be refreshed, the stale-cache refusal, the final reach refresh, a mid-hold CommError, and the new validation.
840545a to
67cefc7
Compare
The abort path now reads the cached position instead of calling _fresh_state(). A zero-gain frame produces no force from q, so a fresh position is not needed to stop the torque; requiring one delayed the release by up to three 50 ms retries and, when the motor did not answer, raised CommError without sending a single frame -- exactly when a stop is needed most.
|
Requested a review from qwtiger. For context on the added commit: |
|
@qwtiger could you take this one? CI is green on 3.10 and 3.14, and the only remaining blocker is the stale changes-requested review. Commit |
|
🎉 This PR is included in version 1.3.1 🎉 The release is available on GitHub release Your semantic-release bot 📦🚀 |
Summary
Source-only fix for the stale-state and grasp-handover paths found on the
bench on 2026-11-18. This supersedes #85.
Changes
MotorController.poll()drains every queued status frame, bounded atMAX_DRAIN_FRAMES, and lets the newest one define the state.poll_until()accepts the update based on the target'srx_count, soanother registered motor's frame cannot hide it.
GripperActions._fresh_state()requests a status frame (0xCC), retries,and refuses to plan when no frame answers; the final reach check uses the
same path instead of a cache read.
below
MotionConfig.stall_speed_mm_s(2 mm/s).CommErroris converted toGraspResult(ok=False)after azero-torque release, so the last hold torque is not left on the bus.
stall_speed_mm_sis validated as a non-negative finite number; the polldrain is capped.
rule, the bounded drain, multi-motor
poll_until, the slow creep thatmust not be handed over, an idle bus that must be refreshed, the
stale-cache refusal, the final reach refresh, a mid-hold
CommError, andthe new validation.
refusal path.
Why
On the bench, a
grasp(5 N, hold_s=2)left the last hold torque on the bus.The status cache still held the position from the start of the hold (78 mm)
while the jaws were at 26 mm; a following
open()scheduled its ramp fromthe cache, commanded ~0.7 rad of error on the first frame, and the jaws left
at 23 rad/s out of a 20 mm/s move.
Separately, a force-budgeted approach that crawls (about 8 mm/s against a
50 mm/s schedule) was classified as stalled by the 20 % window criterion and
handed to the torque-only hold while still in free travel; the hold then
accelerated the jaws across the stroke.
Behaviour
frame;
timeout_sonly applies to the wait for that first frame.the 0xCC request, the call raises
CommErrorinstead of guessing.reached/okdecision also requires an answered frame.open/close/goto/ release and the ordinary 20 % stall criterionare otherwise unchanged.
MotionConfig.stall_speed_mm_sis new (default 2 mm/s); it is a policythreshold, not a measured hardware constant.
Testing
python3 -m unittest discover -s tests -t tests: 511 tests pass.npx --yes markdownlint-cli2@0.23.3 README.md README.zh-CN.md: 0 issues.the updated branch.
Known limitation
A free creep below 2 mm/s can still be mistaken for a stop. It is
reproducible at 2.5 N: the approach creeps at about 1.9 mm/s and the hold
takes over before the closed stop. The threshold stays at 2 mm/s because
2.5 N is below the documented 3 N setpoint (the jaws may not close below
about 2.2 N and are marginal between 2.2 N and 3 N), so this is an
out-of-policy operating point; within the supported range the gate keeps at
least a 2x margin (a 5 N approach crawls at about 8 mm/s).
Issues
Refs #85 (superseded).